package anyVehicle;



import javax.vecmath.Tuple3f;
import javax.vecmath.Vector3f;

import net.minecraft.src.Entity;

import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.dispatch.CollisionObject;
import com.bulletphysics.collision.dispatch.CollisionWorld;
import com.bulletphysics.collision.dispatch.GhostObject;
import com.bulletphysics.collision.dispatch.PairCachingGhostObject;
import com.bulletphysics.collision.shapes.ConvexShape;
import com.bulletphysics.dynamics.character.KinematicCharacterController;
import com.bulletphysics.linearmath.Transform;

public class EntityKinematicController extends KinematicCharacterController {
	private static Vector3f[] upAxisDirection = new Vector3f[] { new Vector3f(1.0f, 0.0f, 0.0f), new Vector3f(0.0f, 1.0f, 0.0f),
			new Vector3f(0.0f, 0.0f, 1.0f), };

	private Entity entity;
	
	private Vector3f previousPos = new Vector3f();

	private boolean hasCollided = false;

	public boolean hasCollided() {
		return hasCollided;
	}

	private static Vector3f tmpVector = new Vector3f();

	private static Transform tmpTrans;

	public EntityKinematicController(Entity entity, PairCachingGhostObject ghostObject, ConvexShape convexShape, float stepHeight) {
		super(ghostObject, convexShape, stepHeight);
		this.entity = entity;
		previousPos.set((float)entity.posX, (float)entity.posY, (float)entity.posZ);
		// currentStepOffset = 0f;
		// stepHeight = 0f;

	}

	public Entity getEntity() {
		return entity;
	}

	public GhostObject getGhostObject() {
		return ghostObject;
	}

	@Override
	protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3f walkMove) {
		// printf("m_normalizedDirection=%f,%f,%f\n",
		// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
		// phase 2: forward and strafe
		Transform start = new Transform();
		Transform end = new Transform();
		targetPosition.add(currentPosition, walkMove);
		start.setIdentity();
		end.setIdentity();

		float fraction = 1.0f;
		Vector3f distance2Vec = new Vector3f();
		distance2Vec.sub(currentPosition, targetPosition);
		float distance2 = distance2Vec.lengthSquared();
		// printf("distance2=%f\n",distance2);

		/*
		 * if (touchingContact) { if (normalizedDirection.dot(touchingNormal) >
		 * 0.0f) { updateTargetPositionBasedOnCollision(touchingNormal); } }
		 */

		int maxIter = 10;

		while (fraction > 0.01f && maxIter-- > 0) {
			start.origin.set(currentPosition);
			end.origin.set(targetPosition);

			KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, upAxisDirection[upAxis], -1.0f);
			callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
			callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;

			float margin = convexShape.getMargin();
			convexShape.setMargin(margin + addedMargin);

			if (useGhostObjectSweepTest) {
				ghostObject.convexSweepTest(convexShape, start, end, callback, collisionWorld.getDispatchInfo().allowedCcdPenetration);
			} else {
				collisionWorld.convexSweepTest(convexShape, start, end, callback);
			}

			convexShape.setMargin(margin);

			fraction -= callback.closestHitFraction;
			
			
			if (callback.hasHit()) {
				hasCollided = true;
				// we moved only a fraction
				Vector3f hitDistanceVec = new Vector3f();
				hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
				// float hitDistance = hitDistanceVec.length();

				// if the distance is farther than the collision margin, move
				// if (hitDistance > addedMargin) {
				// //printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
				// currentPosition.interpolate(currentPosition, targetPosition,
				// callback.closestHitFraction);
				// }

				updateTargetPositionBasedOnCollision(callback.hitNormalWorld);

				Vector3f currentDir = new Vector3f();
				currentDir.sub(targetPosition, currentPosition);
				distance2 = currentDir.lengthSquared();
				if (distance2 > BulletGlobals.SIMD_EPSILON) {
					currentDir.normalize();
					// see Quake2:
					// "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners."
					if (currentDir.dot(normalizedDirection) <= 0.0f) {
						break;
					}
				} else {
					// printf("currentDir: don't normalize a zero vector\n");
					break;
				}
			} else {
				// we moved whole way
				currentPosition.set(targetPosition);
			}

			// if (callback.m_closestHitFraction == 0.f)
			// break;
		}
	}

	private static class KinematicClosestNotMeConvexResultCallback extends CollisionWorld.ClosestConvexResultCallback {
		protected CollisionObject me;
		protected final Vector3f up;
		protected float minSlopeDot;

		public KinematicClosestNotMeConvexResultCallback(CollisionObject me, final Vector3f up, float minSlopeDot) {
			super(new Vector3f(), new Vector3f());
			this.me = me;
			this.up = up;
			this.minSlopeDot = minSlopeDot;
		}

		@Override
		public float addSingleResult(CollisionWorld.LocalConvexResult convexResult, boolean normalInWorldSpace) {
			if (convexResult.hitCollisionObject == me) {
				return 1.0f;
			}

			Vector3f hitNormalWorld;
			if (normalInWorldSpace) {
				hitNormalWorld = convexResult.hitNormalLocal;
			} else {
				// need to transform normal into worldspace
				hitNormalWorld = new Vector3f();
				hitCollisionObject.getWorldTransform(new Transform()).basis.transform(convexResult.hitNormalLocal, hitNormalWorld);
			}

			float dotUp = up.dot(hitNormalWorld);
			if (dotUp < minSlopeDot) {
				return 1.0f;
			}

			return super.addSingleResult(convexResult, normalInWorldSpace);
		}
	}

	public void updateKinematicControllerFromEntity() {
		
		warp(previousPos);
//		tmpVector.set((float) entity.motionX, (float) entity.motionY , (float) entity.motionZ);
		tmpVector.set((float) entity.posX, (float) entity.posY - entity.yOffset + entity.ySize +1f, (float) entity.posZ);
		tmpVector.scaleAdd(-1f, previousPos);  // To calculate change in position since last tick
		tmpVector.scale(JBulletSimulation.FIXED_SIM_STEP_SCALE);
		setWalkDirection(tmpVector);
		previousPos.set((float)entity.posX, (float) entity.posY - entity.yOffset + entity.ySize +1f, (float)entity.posZ);
	}

	public void updateEntityFromKinematicController() {
		tmpTrans = new Transform();
		ghostObject.getWorldTransform(tmpTrans);
		entity.setPosition(VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, tmpTrans.origin.x, entity.posX),
				VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, tmpTrans.origin.y + entity.yOffset - entity.ySize - 1f, entity.posY),
				VehicleUtil.assignIfDifferenceIsGreaterThan(1e-4, tmpTrans.origin.z, entity.posZ));
	}
	
	public Vector3f getPosition(Vector3f pos) {
		pos.set(ghostObject.getWorldTransform(new Transform()).origin);
		return pos;
	}
	
	public void collisionReset() {
		hasCollided = false;
	}

}